# Untitled - By: Admin - Sun Mar 17 2024

import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from machine import SPI
from ubluetooth import CARSTATE,UBLUETOOTH
from umotor import UMOTOR
from pid import PID
from ultrasonic import ULTRASONIC
###################################用户调整参数start############################################
pan_servo_offset=-5   #由于安装误差会引起左右舵机90度不在中位，因此要加上偏差角
global claw_offset   #定义机械爪中心和摄像头图像中心偏移位置
global img #图像缓存，可重复调用
claw_offset=(-20)       #摄像头安装位置带来的目标位置纵向偏差，由于摄像头与爪子中心存在垂直距离要做一个偏差计算
claw_release=30         #爪子释放角度
claw_catch=90          #爪子抓取夹紧角度
pan_servo_start=90+pan_servo_offset      #左右舵机初始位置，正前方
pan_servo_stop=-90      #左右舵机结束，放小球位置
tilt_servo_start=35     #上下舵机初始位置
tilt_servo_stop=-10      #上下舵机end位置
target_title_angle=75   #目标抓取位置，即上下控制舵机抓取小球时的合适位置，具体可以根据上下舵机的安装位置调整，一般安装时选择向上45度作为中位，避免向下运动太多导致机械臂接触到地面上。
DIS_RATE=27000          #测距离系数，使用图像像素多少估算距离
global flag_lost
flag_lost=0                 #用于确认是否达到了可抓取的位置，一般确认多次已保证准确性

#控制舵机需要输出2.5%-12.5%的PWM
tim = Timer(2, freq=50) # Frequency in 50Hz(20ms)
claw=tim.channel(3, Timer.PWM, pin=Pin("A2"), pulse_width_percent=7.5)   #初始角度90度

tracing_serangle_pan=90 #横向舵机角度
tracing_serangle_tilt=60 #纵向舵机角度，调节视野
car_speed=70  #电机目标速度

pan_servo=Servo(1)      #左右控制
tilt_servo=Servo(2)     #上下控制
 #舵机角度矫正，openmv底层驱动问题
pan_servo.calibration(500,2500,1500)#实际底层计算的paulse是50，250，150，对应0-180度
tilt_servo.calibration(500,2500,1500) #实际底层计算的paulse是50，250，150，对应0-180度
#PID 云台PID,小车跟随PID，小车循迹PID
pan_pid  = PID(p=0.1, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.06, i=0.02,d=0.003, imax=90) #脱机运行或者禁用图像传输，使用这个PID
dis_pid  = PID(p=0.9, i=0.01) #小车前后追球PID
len_pid  = PID(p=0.9 ,i=0.01) #小车左右追踪pid
rho_pid = PID(p=1.2, i=0.5)
theta_pid = PID(p=0.6, i=0.2)

#配合视频小车app，实现小车寻迹，跟随，避障三种功能
#寻迹，小车沿着黑线前进，可以自己标定一下线的阈值
#跟随，小车颜色识别跟随，默认是自动捡黄色的球
#避障，增加超声波模块自动捡球避障
#前面三种模式下，按下小车前后左右键可以控制小车
#开灯，关灯按钮暂时用来作为机械臂控制功能，即按下这两个按钮时，上下左右键可以控制云台，
#以上仅为参考功能设计，可以在ubluetooth.py库自行定义其他功能
global ultradis,carstate,cardir,left_speed,right_speed,img,THRESHOLD
ultradis=0.0
motor=UMOTOR()
ble=UBLUETOOTH(uart_port=1,baudrate=9600)
ultrawave = ULTRASONIC(trig_pin="B15",echo_pin="B10")
objects = list()
last_objects=objects

#定义机械爪舵机运动角度
#输入：舵机运动到目标角度
#输出：无
def claw_angle(servo_angle):
    if servo_angle<=0:
        servo_angle=0
    if servo_angle>=180:
        servo_angle=180
    percent=(servo_angle+45)/18
    claw.pulse_width_percent(percent)
#定义舵机运动
#输入：舵机运动的起止角度
#输出：无
def servo_move(servo_n,start_angle,end_angle):
    if start_angle>=end_angle :
        while (start_angle>=end_angle) :
            servo_n.angle(start_angle-2)
            time.sleep(50)
            start_angle=start_angle-2
    return 0
    if start_angle<=end_angle :
        while (start_angle<=end_angle) :
            servo_n.angle(start_angle+2)
            time.sleep(50)
            start_angle=start_angle+2
    return 0

#功能： 寻找最大的目标，计算方式为像素面积排序
#输入： 链表objects，坐标X,坐标y,像素宽,像素高,mode=0是色块，1是人脸坐标
#输出： 最大目标元组,
def find_max_object(objects,mode=0):
    max_size=0
    if mode==0:#色块元组
        max_object=None
    if mode==1:#人脸框坐标格式和色块不同，单独定义
        max_object=(0,0,0,0,0.0)
    for object in objects:
        if object[2]*object[3] > max_size:
            max_object=object
            max_size = object[2]*object[3]
    return max_object

#功能： 切换像素格式，颜色识别用彩色，人脸跟随，寻迹用灰度图
#输入： img，s_flag切换标志
#输出： 无
def sensor_judeg_mode(img,s_flag):
    if  s_flag==1:  #寻迹，灰度图
        s_flag=0
        sensor.set_pixformat(sensor.GRAYSCALE) # use 灰度图像
        img = sensor.snapshot()
    elif s_flag==2 or s_flag==3 : #颜色识别跟随，避障
        sensor.set_pixformat(sensor.RGB565) # use RGB565.彩色图像
        img = sensor.snapshot()
        s_flag=0

#函数功能：自动避障，延时策略很关键
#输入：目标距离 单位：cm
#输出：无
def auto_avoidance(objdis):
    car_speed=70
    if(objdis>= 30):
        motor.run(car_speed,car_speed)      #前进
        time.sleep(100) #前进时间不能长，否则正前方行驶容易一直前进后退
    if objdis<=20 and (objdis>2.0):
        motor.run(-car_speed,-car_speed)    #后退
        time.sleep(600)
    if objdis< 30 and (objdis>20):    #右转
        motor.run(car_speed,-car_speed)
        time.sleep(1500)#转弯时间要稍微长一点，特别是墙角位置转时间短了不容易过去

def color_tracing_line(img):
    # 每个roi为(x, y, w, h)，线检测算法将尝试找到每个roi中最大的blob的质心。
    # 然后用不同的权重对质心的x位置求平均值，其中最大的权重分配给靠近图像底部的roi，
    # 较小的权重分配给下一个roi，以此类推。
    THRESHOLD = [(4, 31, -24, 21, -11, 29)]
    pan_servo.angle(tracing_serangle_pan)
    tilt_servo.angle(tracing_serangle_tilt)
    ROIS = [ # [ROI, weight]
            (10, int(img.height()*2/3), img.width(), int(img.height()/3), 0.7), # 你需要为你的应用程序调整权重
            (10, int(img.height()*1/3), img.width(), int(img.height()/3), 0.3), # 取决于你的机器人是如何设置的。
            (10, 0, img.width(), int(img.height()/3), 0.1)
           ]
    #roi代表三个取样区域，（x,y,w,h,weight）,代表左上顶点（x,y）宽高分别为w和h的矩形，
    #weight为当前矩形的权值，roi即把图像横分成三个矩形。
    #三个矩形的阈值要根据实际情况进行调整，离机器人视野最近的矩形权值要最大.
    # Compute the weight divisor (we're computing this so you don't have to make weights add to 1).
    weight_sum = 0 #权值和初始化
    for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.
    #计算权值和。遍历上面的三个矩形，r[4]即每个矩形的权值。
    centroid_sum = 0
    min_x=1000
    max_x=1
    min_y=1000
    max_y=1
    #利用颜色识别分别寻找三个矩形区域内的线段
    for r in ROIS:
        blobs = img.find_blobs(THRESHOLD, roi=r[0:4], merge=True)
        #目标区域找到直线
        if blobs:
            max_blob=find_max_object(objects=blobs,mode=0)
            # 在色块周围画一个矩形。
            img.draw_rectangle(max_blob.rect(),color = (255,255,255), thickness = 2, fill = False)
            # 将此区域的像素数最大的颜色块画矩形和十字形标记出来
            img.draw_cross(max_blob.cx(),max_blob.cy())
            #计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值
            centroid_sum += max_blob.cx() * r[4] # r[4] is the roi weight.
            if r==ROIS[0]:
                min_x=max_blob.cx()
                min_y=max_blob.cy()
            if r==ROIS[2]:
                max_x=max_blob.cx()
                max_y=max_blob.cy()
   # print("%d"%min_x)
   # print("%d"%min_y)

   # print("%d"%max_x)
   # print("%d"%max_y)
     # 将center_pos转换为一个偏角。我们用的是非线性运算，所以越偏离直线，响应越强。
    center_pos = (centroid_sum / weight_sum) # Determine center of line.
    rho_err=0
    rho_err=center_pos-img.width()/2
    print("rho_err: %f" %rho_err )
    theta_err= 0
    # 下面的等式只是计算三角形的角度,将角度输出限制在-45至45度左右。（不完全是-45至45度）。
    theta_err = math.atan((max_x-min_x)/(abs(max_y-min_y)+1))
    #注意计算得到的是弧度值
    theta_err = math.degrees(theta_err)
    print("theta_err: %f" % theta_err)
    rho_output=0
    theta_output=0
    output=0
   # 根据偏角和中线距离差PID控制小车运转
    if (rho_err>-80) and (theta_err>-44.9):
        rho_output = rho_pid.get_pid(rho_err,1)
        theta_output = theta_pid.get_pid(theta_err,1)
        output = rho_output+theta_output
        print("output",output)
        motor.run(car_speed+output, car_speed-output)
    else:
       motor.run(0,0)
       pass


#功能： 抓取动作
#输入： 小球距离,最大面积的小球元组
#输出： 无
def catch_ball(ball_s,title_angle,target_title_angle):
    global count
    #target_title_angle 是防止机械爪怼到地上去了，大概就是爪子取刚刚到地面时的上下舵机角度
    ############################抓取############################
    #抓取得策略是摄像头与夹子呈一定角度安装，让一半得夹子在图像中，目的是方便判断目标是否一直在夹子上
    #然后修订目标在夹子中得位置，当修订好之后再执行夹取动作。
    if(ball_s>=50 and ball_s<=100) or (title_angle>=target_title_angle):
        motor.run(0,0)
        count=count+1
        if count >=5:
            count=0
            motor.run(0,0)
            print("catch_ball")
            #由于小球比较滑，最好是将爪子调整到合适的角度再抓成功率会高一些
            if (title_angle>=target_title_angle):#小球非常近，已经在夹扣内部了
                tilt_servo.angle(target_title_angle-5)
            else:
                tilt_servo.angle(target_title_angle-10)#小球刚到夹口位置，要稍微修正一下舵机角度再夹取
            time.sleep(1000)
            claw_angle(claw_catch)  #夹取
            time.sleep(1000)
            #tilt_servo.angle(tilt_servo_stop,1000)
            servo_move(tilt_servo,tilt_servo.angle(),tilt_servo_stop) #抬起
            time.sleep(1000)
            pan_servo.angle(0)#向后
            time.sleep(1000)
            pan_servo.angle(pan_servo_stop)#向后
            time.sleep(1000)
            claw_angle(claw_release) #释放
            time.sleep(1000)
            pan_servo.angle(0)#向前
            time.sleep(1000)
            pan_servo.angle(pan_servo_start)#向前
            time.sleep(1000)
            tilt_servo.angle(tilt_servo_start)
            #servo_move(tilt_servo,tilt_servo.angle(),tilt_servo_start) #向下
            time.sleep(1000)
            motor.run(-70,-70) #捡完球要后退，可能有多个球在一起，后退了还能在捡剩下的
            time.sleep(2000)
            motor.run(0,0)

#假设距离变化每次控制在30左右，用来控制速度快慢
s_last=0
s_lowpower=0
#速度控制，主要用来控制速度变化快慢，达到匀速效果
def speed_error_control(ball_s,power,high_s,low_s):
    global s_last
    power_s=power
    s_error=s_last-ball_s            #计算两次距离变化，前进时距离在接近
    s_last=ball_s                    #更新s_last
    if s_error>= high_s :            #变化太快
        power_s=power_s-s_error*0.2  #对power进行修正
    if abs(s_error)<low_s:           #变化太慢或基本没有变化
        power_s=power_s+5            #对power进行修正
    return power_s

#功能： 追踪动作
#输入： 图像，小球距离,最大面积的小球元组
#输出： 无
def traking_ball(ball_s,title_angle,target_title_angle,pan_error):
    global count
    if ((ball_s>100) and (ball_s<2000)) and (title_angle<target_title_angle):
        count=0
        claw_angle(claw_release)
        #############################小车追踪############################
        power=0
        power_s=0
        power_l=0
        #前进与后退控制，
        #没有做后退，而是采用先大致接近目标，再修订距离得方式
        #前进与后退控制，这里采用的是像素面积计算距离，而抓取采用的是距离参数，
        #两者可以统一一下，具体可以根据实际物体大小测定后调整
        #小车速度控制在一定范围，保持低速运动抓取成功率高，根据路面不同调整速度范围
        if ball_s>300 and ball_s<=2000:
            dis_error=ball_s-300
            power_s=int(dis_pid.get_pid(dis_error,1)/2)
        elif ball_s>100 and ball_s<=300:
            dis_error=ball_s-100
            power_s=int(dis_pid.get_pid(dis_error,1)/2)
            #电机PWM限幅，防止最后一点距离走不到
        else:
            power_s =50
        power_s=speed_error_control(ball_s,power_s,25,10)
        print("power_s:",power_s)
        if(power_s>80):
            power_s=80
        if(power_s<40):
            power_s=40
        power_l=int(len_pid.get_pid(pan_error,1))
        #print("power_s:",power_s)
        #print("power_l:",power_l)
        motor.run(power_s-power_l,power_s+power_l)

#功能： 小球颜色识别
#输入： 图像
#输出： ball_s,max_blob
def color_detect(img):
    #全局变量
    THRESHOLD  =[ (58, 86, 7, 53, 33, 80),(22, 50, 21, 55, 37, 66)]  #黄色乒乓球# 可以根据白天夜晚光 多增加几组阈值，目前取了白天和晚上灯光下的场景阈值
    global last_objects,objects,flag_lost
    ball_s=0
    max_blob=None
    if last_objects:  #如果上一张图像中找到了色块，就在色块周边扩展，然后在扩展后的ROI区域进行寻找色块，这样有利于追踪
         for b in objects:
            x1 = b[0]-7
            y1 = b[1]-7
            w1 = b[2]+12
            h1 = b[3]+12
         roi2 = (x1,y1,w1,h1)
         objects = img.find_blobs(THRESHOLD,roi = roi2,area_threshold=100,merge=True)
         last_objects = objects
    else:       #如果没有找到色块，就重新计算全图的色块，并将全图色块信息保存到上一张图像上，这样下一次就可以再进行ROI区域寻找色块
        objects = img.find_blobs(THRESHOLD,area_threshold=100,merge=True)
        last_objects = objects
    #step1 有目标，就进行追踪和抓取操作
    if objects:  #如果找到了色块，就计算最大的色块位置，并做舵机追踪
        flag_lost=0
        ball_s=0
        max_blob = find_max_object(objects)
        #色块的位置和大小需要满足图像本身大小的要求，以免越界
        ball_s=DIS_RATE/(max_blob[2]*2) #计算距离
        img.draw_rectangle(max_blob.rect())
        cx=int(max_blob[0]+max_blob[2]/2)
        cy=int(max_blob[1]+max_blob[3]/2)
        img.draw_cross(cx, cy) # cx, cy画中心点
        img.draw_string(cx,cy, "%.2f mm"%(ball_s))  #显示目标的距离参数
    return ball_s,max_blob

#功能： 自动找球控制
#输入： 图像，小球距离,最大面积的小球元组
#输出： 无

def find_ball_auto_control(img,mode=0):
    global flag_lost
    ball_s=0
    ball_s,max_blob=color_detect(img)
    if ball_s>0:
        #step1 计算目标距离图像中心的偏差，做机械臂上下调整达到可抓取位置
        pan_error=0
        tilt_output=0
        title_angle=0
        flag_lost=0
        pan_error = img.width()/2-(max_blob[2]/2+max_blob[0])
        tilt_error = (img.height()/2-claw_offset)-(max_blob[3]/2+max_blob[1])
        #pan_output=pan_pid.get_pid(pan_error,1)/2
        tilt_output=tilt_pid.get_pid(tilt_error,1)
        #关闭左右追踪
        title_angle=tilt_servo.angle()-tilt_output
        #pan_servo.angle(pan_servo.angle()+pan_output)
        tilt_servo.angle(title_angle)
        #print("title_angle: ", title_angle)
        #step2 根据上下舵机的位置判断是否可抓取
        catch_ball(ball_s,title_angle,target_title_angle)
        #step3 如果达不到抓取条件，就自动追踪小球
        traking_ball(ball_s,title_angle,target_title_angle,pan_error)
    else :
        claw_angle(claw_release)
        flag_lost=flag_lost+1
        if mode>0 : #避障模式下才采集超声波数据，否则不不接超声波这里采集数据会很慢
            ultradis=ultrawave.get_distance()
            #print("auto avoidance:%s"%ultradis)
        if flag_lost>5:#连续5帧没有
            flag_lost=0
            if(tilt_servo.angle()>=tilt_servo_start):
                tilt_servo.angle(tilt_servo.angle())
            else :
                tilt_servo.angle(tilt_servo_start)
            if mode>0 :
                #step4 没有找到目标就进行超声波避障前进
                auto_avoidance(ultradis)
            else:
                #tilt_servo.angle(tilt_servo_start)
                #motor.run(70,-70) #原地旋转找球
                #time.sleep(500) #旋转找球每次等一会
                motor.run(0,0) #停止等待目标出现



def car_state_deal(img):
    carstate,cardir,left_speed,right_speed,s_flag=ble.bluetooth_deal()
    sensor_judeg_mode(img,s_flag)
    if (carstate==CARSTATE.enMANUAL): #手动控制小车
        motor.run(left_speed,right_speed)
    elif (carstate==CARSTATE.enFLITING or carstate==CARSTATE.enSTRAKING): #控制舵机
        if(cardir==CARSTATE.enRUN):  #舵机向上
            tilt_servo.angle(tilt_servo.angle()-4)
        elif(cardir==CARSTATE.enBACK): #舵机向下
            tilt_servo.angle(tilt_servo.angle()+4)
        elif(cardir==CARSTATE.enLEFT): #舵机向左
            pan_servo.angle(pan_servo.angle()+4)
        elif(cardir==CARSTATE.enRIGHT):#舵机向右
            pan_servo.angle(pan_servo.angle()-4)
        elif(cardir==CARSTATE.enRELEASE):
            claw_angle(claw_release)
        elif(cardir==CARSTATE.enCATCH):
            claw_angle(claw_catch)
    elif(carstate==CARSTATE.enTRACING):#寻迹
        color_tracing_line(img)
    elif(carstate==CARSTATE.enTRAKING): #自动追踪捡球
        find_ball_auto_control(img,mode=0)
    elif(carstate==CARSTATE.enAVOIDING):#自动捡球避障
        find_ball_auto_control(img,mode=1)


claw_angle(claw_release)
tilt_servo.angle(tilt_servo_start)
pan_servo.angle(pan_servo_start)

sensor.reset()
#sensor.set_contrast(1)
#sensor.set_brightness(1)
sensor.set_framesize(sensor.HQVGA)
#sensor.set_quality(90)
sensor.set_pixformat(sensor.RGB565)
#sensor.set_hmirror(True) #水平镜像，方便调试
#sensor.set_vflip(True) #不同的摄像头需要设置一下旋转镜像才能正常跟踪
sensor.skip_frames(50)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
#sensor.set_pixformat(sensor.JPEG) #OV2640 SET
clock = time.clock()
last_time= time.ticks()
#Auto_exposeure_sensor() #自动曝光

while(True):
  clock.tick()
  img = sensor.snapshot()
  car_state_deal(img=img)
  print(clock.fps(), "fps", end="\n\n")
  gc.collect()
